{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 479,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import seaborn as sns\n",
    "import time\n",
    "from PIL import Image\n",
    "from PIL import ImageDraw\n",
    "import math\n",
    "plt.style.use({'figure.figsize':(10, 10)})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 480,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#constant\n",
    "Epsilon_start=1\n",
    "Epsilon_final=0.01\n",
    "Decay_Rate=0.000001#he dacaying rate of the Epsilon, the range of the epsilon is 0.01-1, initially it is 1.\n",
    "Action_times=0 #Rigister the totality of the times of selecting actions, including the random selections and selection based on Q_Table\n",
    "Velocity_tripod=0.289*40\n",
    "Robot_R=40\n",
    "Beta=0.9\n",
    "Alpha=0.2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 481,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style>\n",
       "    .dataframe thead tr:only-child th {\n",
       "        text-align: right;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: left;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>Up</th>\n",
       "      <th>Down</th>\n",
       "      <th>Left_45D</th>\n",
       "      <th>Right_45D</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>L60D0/30</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D0/30</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D0/30</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D0/30</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D30/60</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>"
      ],
      "text/plain": [
       "            Up  Down  Left_45D  Right_45D\n",
       "L60D0/30   0.0   0.0       0.0        0.0\n",
       "L180D0/30  0.0   0.0       0.0        0.0\n",
       "L250D0/30  0.0   0.0       0.0        0.0\n",
       "L300D0/30  0.0   0.0       0.0        0.0\n",
       "L60D30/60  0.0   0.0       0.0        0.0"
      ]
     },
     "execution_count": 481,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "States2=np.array(['L60D0/30','L180D0/30','L250D0/30','L300D0/30',\n",
    "                'L60D30/60','L180D30/60','L250D30/60','L300D30/60',\n",
    "                 'L60D60/90','L180D60/90','L250D60/90','L300D60/90',\n",
    "                'L60D90/120','L180D90/120','L250D90/120','L300D90/120',\n",
    "                'L60D120/150','L180D120/150','L250D120/1500','L300D120/150',\n",
    "                'L60D150/180','L180D150/180','L250D150/180','L300D150/180',\n",
    "                'L60D180/210','L180D180/210','L250D180/210','L300D180/210',\n",
    "                'L60D210/240','L180D210/240','L250D210/240','L300D210/240',\n",
    "                'L60D240/270','L180D240/270','L250D240/270','L300D240/270',\n",
    "                'L60D270/300','L180D270/300','L250D270/300','L300D270/300',\n",
    "                'L60D300/330','L180D300/330','L250D300/330','L300D300/330',\n",
    "                 'L60D330/360','L180D330/360','L250D330/360','L30D330/360',])\n",
    "Actions2=np.array(['Up','Down','Left_45D','Right_45D'])\n",
    "Actions_len2=len(Actions2)\n",
    "States_len2=len(States2)\n",
    "Q_Table_Real2=np.zeros((States_len2,Actions_len2))\n",
    "Q_Table_Real2=pd.DataFrame(Q_Table_Real2,columns=Actions2,index=States2)\n",
    "Q_Table_Real2.head()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 482,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Set_destination(x_,y_):\n",
    "    return x_,y_\n",
    "des_x,des_y=Set_destination(900,900)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 483,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAlsAAAJCCAYAAAD3HAIiAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nSUVORK5CYII=\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x1b32595b2e8>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "#Draw_map2 funcion is used to train the first Q-Table2, which allows it to arrive to the destination\n",
    "im = Image.new(\"I\", size=(1000,1000),color=0) #Color means the degree of Gray scale, 255 means white, 0 means black\n",
    "draw = ImageDraw.Draw(im,mode='I') #mode=\"I\" means the made degree of Gray scale\n",
    "#Draw the black frame around the rectangle\n",
    "draw.rectangle((40,40,960,960),255,255)  #the last elements means the color of the inner graph and that of the outer frame\n",
    "def Draw_map2_with_dis(x_,y_):\n",
    "    draw.ellipse((x_-10,y_-10,x_+10,y_+10),2, 2) #the color of destination is 2\n",
    "Draw_map2_with_dis(des_x,des_y)\n",
    "plt.imshow(im)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 484,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Initial_Q_Table2(Actions_len2_,States_len2_):\n",
    "    Q_Table2_=np.zeros((States_len2_,Actions_len2_))\n",
    "    print('***********************************************************')\n",
    "    print(\"Succeed to initialize Q-Table2!\")\n",
    "    print('***********************************************************')\n",
    "    return Q_Table2_\n",
    "# Q_Table2=Initial_Q_Table2(Actions_len2,States_len2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 485,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(75.0, 'left', 141.42135623730951)"
      ]
     },
     "execution_count": 485,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "def Robot_desti_(desti_x_,desti_y_,robot_x_,robot_y_,robot_a):\n",
    "    #Left-negative,180-360 |||| Right-positive,0-180\n",
    "    delta_x=desti_x_-robot_x_\n",
    "    delta_y=desti_y_-robot_y_\n",
    "    distance=np.sqrt(delta_x*delta_x+delta_y*delta_y)\n",
    "#     print(delta_x)\n",
    "#     print(delta_y)\n",
    "#     print((np.arctan(delta_y/delta_x))*180/np.pi)\n",
    "    direct_angle=(np.arctan(delta_y/delta_x))*180/np.pi\n",
    "    true_angle=(robot_a-direct_angle)%360\n",
    "#     print(true_angle)\n",
    "    if true_angle>180:\n",
    "        return 360-true_angle,'right',distance\n",
    "    else:\n",
    "        return true_angle,'left',distance\n",
    "(Robot_desti_(300,100,200,200,30))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 486,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#Is_Crash function, in the second map, is used to judge if the robot collides with the black frame\n",
    "def Is_Crash(Current_x_,Current_y_):\n",
    "    Crash_=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Dis=np.arange(0,50,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x_=Current_x_+i*np.cos(j/180*np.pi)\n",
    "            y_=Current_y_+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x_,y_)))==0:\n",
    "                Crash_=True\n",
    "                break\n",
    "        if Crash_==True:\n",
    "                break\n",
    "    return Crash_"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 487,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "False\n"
     ]
    }
   ],
   "source": [
    "def Is_arrive(Current_x_,Current_y_):\n",
    "    Arrive_=False\n",
    "    Crash_=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Dis=np.arange(0,40,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x_=Current_x_+i*np.cos(j/180*np.pi)\n",
    "            y_=Current_y_+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x_,y_)))==0:\n",
    "                Crash_=True\n",
    "                break\n",
    "            if (im.getpixel((x_,y_)))==2:\n",
    "                Arrive_=True\n",
    "                break\n",
    "        if Arrive_==True or Crash_==True:\n",
    "                break\n",
    "    return Arrive_\n",
    "print(Is_arrive(500,500))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 488,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#Random_start function, in the second map, is used to set the beginning point in the white map.\n",
    "def Random_start():\n",
    "    Angle_=0\n",
    "    x_,y_=np.random.random(2)*1000\n",
    "    while(Is_Crash(x_,y_)==True or Is_arrive(x_,y_)==True):\n",
    "#         print('Boom')\n",
    "        x_,y_=np.random.random(2)*1000\n",
    "    return x_,y_,Angle_\n",
    "#print(Random_start())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 489,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Output_state_index(true_angle,left_right,distance_):\n",
    "    if 0<=distance_<60:\n",
    "        dis_level=0\n",
    "    elif 60<=distance_<180:\n",
    "        dis_level=1\n",
    "    elif 180<=distance_<250:\n",
    "        dis_level=2\n",
    "    else:\n",
    "        dis_level=3\n",
    "\n",
    "    if left_right=='left':\n",
    "        le_ri=0\n",
    "    elif left_right=='right':\n",
    "        le_ri=1\n",
    "    \n",
    "    angle_level=true_angle//30\n",
    "    \n",
    "    State_number=int(dis_level+24*le_ri+angle_level*4)\n",
    "    \n",
    "    return State_number,dis_level,angle_level\n",
    "# State_Number=Output_state_index(179,'right',300) #180 belongs to 180-210,''right' does not have 180\n",
    "# States2[State_Number]\n",
    "    \n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 490,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Choose_action2(Q_Table_,Current_state_,Action_times_):\n",
    "    Epsilon=Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_Rate*Action_times_)\n",
    "    State_action_=Q_Table_[Current_state_,:]\n",
    "    if(np.random.random()<Epsilon or np.all(State_action_==[0])):\n",
    "        Action_next=np.random.randint(Actions_len2)\n",
    "    else:\n",
    "        Action_next=np.argmax(State_action_)\n",
    "    return Action_next\n",
    "# print(Choose_action2(Q_Table2,20,0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 491,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Next_state_F2(Robot_x_,Robot_y_,Robot_a_,Desti_x_,Desti_y_,Action_):\n",
    "    Arrive_=False\n",
    "    Crash_=False\n",
    "    if Action_==0:\n",
    "        Ro_next_x=Robot_x_+Velocity_tripod*np.cos(Robot_a_/180*np.pi)\n",
    "        Ro_next_y=Robot_y_+Velocity_tripod*np.sin(Robot_a_/180*np.pi)\n",
    "        if Is_Crash(Ro_next_x,Ro_next_y)==True:\n",
    "#             print(\"Boom!\")\n",
    "            Crash_=True\n",
    "        if Is_arrive(Ro_next_x,Ro_next_y)==True:\n",
    "            Arrive_=True\n",
    "    elif Action_==1:\n",
    "        Ro_next_x=Robot_x_-Velocity_tripod*np.cos(Robot_a_/180*np.pi)\n",
    "        Ro_next_y=Robot_y_-Velocity_tripod*np.sin(Robot_a_/180*np.pi)\n",
    "        if Is_Crash(Ro_next_x,Ro_next_y)==True:\n",
    "#             print(\"Boom!\")\n",
    "            Crash_=True\n",
    "        if Is_arrive(Ro_next_x,Ro_next_y)==True:\n",
    "            Arrive_=True\n",
    "    elif Action_==2:\n",
    "        Robot_a_=Robot_a_-45\n",
    "        Ro_next_x=Robot_x_\n",
    "        Ro_next_y=Robot_y_\n",
    "    elif Action_==3:\n",
    "        Robot_a_=Robot_a_+45\n",
    "        Ro_next_x=Robot_x_\n",
    "        Ro_next_y=Robot_y_\n",
    "    True_angle_,Le_ri_,Distance_=Robot_desti_(Desti_x_,Desti_y_,Robot_x_,Robot_y_,Robot_a_)\n",
    "    State_Number_,Distance_level,Angle_level=Output_state_index(True_angle_,Le_ri_,Distance_)\n",
    "    if Arrive_==False:\n",
    "        R=Distance_level*-1+Angle_level*-(0.5)\n",
    "    elif Arrive_==True:\n",
    "        R=500\n",
    "    return Ro_next_x,Ro_next_y,Robot_a_,R,State_Number_,Arrive_,Crash_\n",
    "#Next_state_F2(600,600,50,1800,1800,1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 492,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Plot_Move(Vec_x_,Vec_y_,Destination_x_,Destination_y_):\n",
    "    im1=Image.new(\"I\", size=(1000,1000),color=0)\n",
    "    draw1 = ImageDraw.Draw(im1,mode='I')\n",
    "    draw1.rectangle((40,40,960,960),255,255)\n",
    "    draw1.ellipse((Destination_x_-10,Destination_y_-10,Destination_x_+10,Destination_y_+10),2, 2)\n",
    "    for i,j in zip(Vec_x_,Vec_y_):\n",
    "        draw1.ellipse((i-40,j-40, i+40,j+40),150,150) \n",
    "    plt.imshow(im1)\n",
    "    plt.show()\n",
    "    del draw1\n",
    "    del im1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 493,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "***********************************************************\n",
      "Succeed to initialize Q-Table2!\n",
      "***********************************************************\n"
     ]
    }
   ],
   "source": [
    "Q_Table2=Initial_Q_Table2(Actions_len2,States_len2)\n",
    "global Epoche\n",
    "Epoche=0\n",
    "global Action_times #Rigister the totality of the times of selecting actions, including the random selections and selection based on Q_Table\n",
    "Action_times=0\n",
    "global Epoche_action_interval"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 494,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "**********************************\n",
      "last_x=868.141688, last_y=870.783356,last_a=270.000000\n",
      "x=868.141688 y=882.343356 True_angle=118.996276 Des_Ro_dis=36.424018 Action=1 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=28.996276 Robot_a=270\n",
      "0.959652324243\n",
      "41609\n",
      "**********************************\n",
      "last_x=873.565189, last_y=858.726340,last_a=45.000000\n",
      "x=881.739343 y=866.900494 True_angle=16.114930 Des_Ro_dis=37.802498 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=61.114930 Robot_a=45\n",
      "0.959137752145\n",
      "42151\n",
      "**********************************\n",
      "last_x=866.160856, last_y=856.656677,last_a=45.000000\n",
      "x=874.335010 y=864.830832 True_angle=8.879594 Des_Ro_dis=43.538053 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=53.879594 Robot_a=45\n",
      "0.910428663876\n",
      "94834\n",
      "**********************************\n",
      "last_x=854.393112, last_y=907.399354,last_a=315.000000\n",
      "x=862.567267 y=899.225200 True_angle=46.185765 Des_Ro_dis=37.440751 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=1.185765 Robot_a=315\n",
      "0.892043119495\n",
      "115464\n",
      "**********************************\n",
      "last_x=878.704494, last_y=851.052716,last_a=90.000000\n",
      "x=878.704494 y=862.612716 True_angle=29.665503 Des_Ro_dis=43.026824 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=60.334497 Robot_a=90\n",
      "0.891336885841\n",
      "116265\n",
      "**********************************\n",
      "last_x=890.402140, last_y=852.318862,last_a=90.000000\n",
      "x=890.402140 y=863.878862 True_angle=14.880398 Des_Ro_dis=37.374530 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=75.119602 Robot_a=90\n",
      "0.859274914038\n",
      "153322\n",
      "**********************************\n",
      "last_x=902.131183, last_y=846.017059,last_a=90.000000\n",
      "x=902.131183 y=857.577059 True_angle=177.124074 Des_Ro_dis=42.476439 Action=0 S_N=20\n",
      "L60D150/180\n",
      "Des_Robot_a=-87.124074 Robot_a=90\n",
      "0.850268045843\n",
      "163984\n",
      "**********************************\n",
      "last_x=872.581223, last_y=852.444958,last_a=45.000000\n",
      "x=880.755378 y=860.619112 True_angle=18.956200 Des_Ro_dis=43.831607 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=63.956200 Robot_a=45\n",
      "0.832404151837\n",
      "185473\n",
      "**********************************\n",
      "last_x=858.342220, last_y=914.607580,last_a=0.000000\n",
      "x=869.902220 y=914.607580 True_angle=25.889036 Des_Ro_dis=33.455310 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-25.889036 Robot_a=0\n",
      "0.831817164852\n",
      "186187\n",
      "**********************************\n",
      "last_x=899.261680, last_y=851.520911,last_a=90.000000\n",
      "x=899.261680 y=863.080911 True_angle=1.145667 Des_Ro_dis=36.926470 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=88.854333 Robot_a=90\n",
      "0.828462797006\n",
      "190277\n",
      "**********************************\n",
      "last_x=896.753274, last_y=854.906461,last_a=45.000000\n",
      "x=904.927428 y=863.080615 True_angle=127.397972 Des_Ro_dis=37.246752 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=-82.397972 Robot_a=45\n",
      "0.822120674575\n",
      "198056\n",
      "**********************************\n",
      "last_x=895.795856, last_y=855.191247,last_a=45.000000\n",
      "x=903.970010 y=863.365402 True_angle=128.815118 Des_Ro_dis=36.849081 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=-83.815118 Robot_a=45\n",
      "0.819715493302\n",
      "201022\n",
      "**********************************\n",
      "last_x=913.323226, last_y=849.842028,last_a=270.000000\n",
      "x=913.323226 y=861.402028 True_angle=19.043621 Des_Ro_dis=40.832729 Action=1 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=-70.956379 Robot_a=270\n",
      "0.808151910309\n",
      "215406\n",
      "**********************************\n",
      "last_x=907.405544, last_y=856.665925,last_a=45.000000\n",
      "x=915.579698 y=864.840079 True_angle=111.101409 Des_Ro_dis=38.457080 Action=0 S_N=16\n",
      "L60D120/150\n",
      "Des_Robot_a=-66.101409 Robot_a=45\n",
      "0.807302331006\n",
      "216471\n",
      "**********************************\n",
      "last_x=870.440372, last_y=856.054255,last_a=45.000000\n",
      "x=878.614527 y=864.228409 True_angle=14.127583 Des_Ro_dis=41.676674 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=59.127583 Robot_a=45\n",
      "0.80347889569\n",
      "221278\n",
      "**********************************\n",
      "last_x=865.439906, last_y=871.472107,last_a=45.000000\n",
      "x=873.614060 y=879.646261 True_angle=7.353917 Des_Ro_dis=33.324053 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=37.646083 Robot_a=45\n",
      "0.800395887318\n",
      "225171\n",
      "**********************************\n",
      "last_x=889.262334, last_y=857.960865,last_a=90.000000\n",
      "x=889.262334 y=869.520865 True_angle=19.407138 Des_Ro_dis=32.315246 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=70.592862 Robot_a=90\n",
      "0.799645367779\n",
      "226121\n",
      "**********************************\n",
      "last_x=894.705533, last_y=852.256582,last_a=270.000000\n",
      "x=894.705533 y=863.816582 True_angle=171.675385 Des_Ro_dis=36.568718 Action=1 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=81.675385 Robot_a=270\n",
      "0.798969720562\n",
      "226977\n",
      "**********************************\n",
      "last_x=852.816647, last_y=903.806696,last_a=315.000000\n",
      "x=860.990801 y=895.632541 True_angle=51.388216 Des_Ro_dis=39.252927 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=6.388216 Robot_a=315\n",
      "0.796014266579\n",
      "230730\n",
      "**********************************\n",
      "last_x=846.413780, last_y=893.006745,last_a=0.000000\n",
      "x=857.973780 y=893.006745 True_angle=9.447578 Des_Ro_dis=42.604093 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=9.447578 Robot_a=0\n",
      "0.792910153664\n",
      "234687\n",
      "**********************************\n",
      "last_x=856.266321, last_y=879.025253,last_a=0.000000\n",
      "x=867.826321 y=879.025253 True_angle=33.101237 Des_Ro_dis=38.406843 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=33.101237 Robot_a=0\n",
      "0.792446025571\n",
      "235280\n",
      "**********************************\n",
      "last_x=900.313937, last_y=853.325618,last_a=45.000000\n",
      "x=908.488091 y=861.499772 True_angle=122.566975 Des_Ro_dis=39.424805 Action=0 S_N=16\n",
      "L60D120/150\n",
      "Des_Robot_a=-77.566975 Robot_a=45\n",
      "0.791810937279\n",
      "236092\n",
      "**********************************\n",
      "last_x=883.319238, last_y=859.618726,last_a=90.000000\n",
      "x=883.319238 y=871.178726 True_angle=30.060783 Des_Ro_dis=33.300355 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=59.939217 Robot_a=90\n",
      "0.790227031321\n",
      "238120\n",
      "**********************************\n",
      "last_x=852.238098, last_y=887.856705,last_a=0.000000\n",
      "x=863.798098 y=887.856705 True_angle=18.543128 Des_Ro_dis=38.184255 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=18.543128 Robot_a=0\n",
      "0.789168981289\n",
      "239477\n",
      "**********************************\n",
      "last_x=892.137608, last_y=857.800973,last_a=135.000000\n",
      "x=883.963454 y=865.975127 True_angle=70.235361 Des_Ro_dis=37.614662 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=64.764639 Robot_a=135\n",
      "0.786886249902\n",
      "242411\n",
      "**********************************\n",
      "last_x=883.656083, last_y=858.385453,last_a=270.000000\n",
      "x=883.656083 y=869.945453 True_angle=151.462278 Des_Ro_dis=34.211101 Action=1 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=61.462278 Robot_a=270\n",
      "0.786359699546\n",
      "243089\n",
      "**********************************\n",
      "last_x=857.613875, last_y=877.834326,last_a=45.000000\n",
      "x=865.788029 y=886.008481 True_angle=22.757127 Des_Ro_dis=36.962434 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=22.242873 Robot_a=45\n",
      "0.78436004754\n",
      "245668\n",
      "**********************************\n",
      "last_x=888.069725, last_y=855.683298,last_a=45.000000\n",
      "x=896.243880 y=863.857452 True_angle=39.066826 Des_Ro_dis=36.337201 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=84.066826 Robot_a=45\n",
      "0.782571022555\n",
      "247981\n",
      "**********************************\n",
      "last_x=851.707544, last_y=876.320906,last_a=45.000000\n",
      "x=859.881699 y=884.495061 True_angle=23.869457 Des_Ro_dis=43.010246 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=21.130543 Robot_a=45\n",
      "0.780738343544\n",
      "250356\n",
      "**********************************\n",
      "last_x=894.528118, last_y=851.820686,last_a=45.000000\n",
      "x=902.702272 y=859.994841 True_angle=131.135650 Des_Ro_dis=40.096322 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=-86.135650 Robot_a=45\n",
      "0.778824667707\n",
      "252842\n",
      "**********************************\n",
      "last_x=864.904466, last_y=865.179105,last_a=45.000000\n",
      "x=873.078620 y=873.353260 True_angle=0.293748 Des_Ro_dis=37.878879 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=44.706252 Robot_a=45\n",
      "0.776724805112\n",
      "255577\n",
      "**********************************\n",
      "last_x=856.516547, last_y=891.370275,last_a=0.000000\n",
      "x=868.076547 y=891.370275 True_angle=15.126960 Des_Ro_dis=33.069306 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=15.126960 Robot_a=0\n",
      "0.772842770939\n",
      "260653\n",
      "**********************************\n",
      "last_x=871.953240, last_y=861.146805,last_a=45.000000\n",
      "x=880.127395 y=869.320959 True_angle=12.066514 Des_Ro_dis=36.553030 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=57.066514 Robot_a=45\n",
      "0.772001819233\n",
      "261756\n",
      "**********************************\n",
      "last_x=884.632113, last_y=858.629048,last_a=135.000000\n",
      "x=876.457959 y=866.803203 True_angle=80.342971 Des_Ro_dis=40.697113 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=54.657029 Robot_a=135\n",
      "0.771528001474\n",
      "262378\n",
      "**********************************\n",
      "last_x=856.595015, last_y=890.101345,last_a=0.000000\n",
      "x=868.155015 y=890.101345 True_angle=17.267269 Des_Ro_dis=33.347960 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=17.267269 Robot_a=0\n",
      "0.768459910041\n",
      "266415\n",
      "**********************************\n",
      "last_x=889.850637, last_y=858.163386,last_a=45.000000\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "x=898.024791 y=866.337540 True_angle=41.641912 Des_Ro_dis=33.720360 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=86.641912 Robot_a=45\n",
      "0.767140580148\n",
      "268156\n",
      "**********************************\n",
      "last_x=871.093853, last_y=862.955216,last_a=45.000000\n",
      "x=879.268008 y=871.129371 True_angle=9.317881 Des_Ro_dis=35.543336 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=54.317881 Robot_a=45\n",
      "0.766701565938\n",
      "268736\n",
      "**********************************\n",
      "last_x=849.490691, last_y=887.547910,last_a=0.000000\n",
      "x=861.050691 y=887.547910 True_angle=17.729072 Des_Ro_dis=40.891359 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=17.729072 Robot_a=0\n",
      "0.766027645119\n",
      "269627\n",
      "**********************************\n",
      "last_x=871.913697, last_y=865.691343,last_a=0.000000\n",
      "x=883.473697 y=865.691343 True_angle=64.280124 Des_Ro_dis=38.081526 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=64.280124 Robot_a=0\n",
      "0.765159468218\n",
      "270776\n",
      "**********************************\n",
      "last_x=863.551084, last_y=864.180887,last_a=0.000000\n",
      "x=875.111084 y=864.180887 True_angle=55.206489 Des_Ro_dis=43.617279 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=55.206489 Robot_a=0\n",
      "0.764542000352\n",
      "271594\n",
      "**********************************\n",
      "last_x=861.763992, last_y=867.228227,last_a=0.000000\n",
      "x=873.323992 y=867.228227 True_angle=50.854637 Des_Ro_dis=42.256343 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=50.854637 Robot_a=0\n",
      "0.764333775496\n",
      "271870\n",
      "**********************************\n",
      "last_x=857.807200, last_y=880.505549,last_a=45.000000\n",
      "x=865.981354 y=888.679703 True_angle=26.594239 Des_Ro_dis=35.852718 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=18.405761 Robot_a=45\n",
      "0.764060756089\n",
      "272232\n",
      "**********************************\n",
      "last_x=903.948400, last_y=845.091819,last_a=270.000000\n",
      "x=903.948400 y=856.651819 True_angle=5.204466 Des_Ro_dis=43.527631 Action=1 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=-84.795534 Robot_a=270\n",
      "0.763546661979\n",
      "272914\n",
      "**********************************\n",
      "last_x=851.273311, last_y=894.907285,last_a=45.000000\n",
      "x=859.447466 y=903.081439 True_angle=49.345347 Des_Ro_dis=40.669440 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=-4.345347 Robot_a=45\n",
      "0.763542894255\n",
      "272919\n",
      "**********************************\n",
      "last_x=849.964343, last_y=887.182260,last_a=0.000000\n",
      "x=861.524343 y=887.182260 True_angle=18.424925 Des_Ro_dis=40.554539 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=18.424925 Robot_a=0\n",
      "0.762763380827\n",
      "273954\n",
      "**********************************\n",
      "last_x=865.592124, last_y=872.083874,last_a=45.000000\n",
      "x=873.766278 y=880.258028 True_angle=8.037010 Des_Ro_dis=32.832204 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=36.962990 Robot_a=45\n",
      "0.762117034729\n",
      "274813\n",
      "**********************************\n",
      "last_x=867.209466, last_y=861.686476,last_a=45.000000\n",
      "x=875.383620 y=869.860631 True_angle=5.759712 Des_Ro_dis=38.914621 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=50.759712 Robot_a=45\n",
      "0.761717766598\n",
      "275344\n",
      "**********************************\n",
      "last_x=853.513171, last_y=915.760400,last_a=0.000000\n",
      "x=865.073171 y=915.760400 True_angle=24.286825 Des_Ro_dis=38.318058 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-24.286825 Robot_a=0\n",
      "0.761132406485\n",
      "276123\n",
      "**********************************\n",
      "last_x=888.975435, last_y=860.655620,last_a=45.000000\n",
      "x=897.149590 y=868.829774 True_angle=39.775029 Des_Ro_dis=31.300285 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=84.775029 Robot_a=45\n",
      "0.760688618386\n",
      "276714\n",
      "**********************************\n",
      "last_x=856.119867, last_y=880.273501,last_a=45.000000\n",
      "x=864.294022 y=888.447655 True_angle=27.071483 Des_Ro_dis=37.528303 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=17.928517 Robot_a=45\n",
      "0.760533992457\n",
      "276920\n",
      "**********************************\n",
      "last_x=871.742144, last_y=866.846408,last_a=135.000000\n",
      "x=863.567989 y=875.020562 True_angle=100.563698 Des_Ro_dis=44.173111 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=34.436302 Robot_a=135\n",
      "0.760238340311\n",
      "277314\n",
      "**********************************\n",
      "last_x=858.794275, last_y=873.757265,last_a=45.000000\n",
      "x=866.968429 y=881.931419 True_angle=16.320924 Des_Ro_dis=37.650475 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=28.679076 Robot_a=45\n",
      "0.759733599773\n",
      "277987\n",
      "**********************************\n",
      "last_x=862.432246, last_y=865.267011,last_a=0.000000\n",
      "x=873.992246 y=865.267011 True_angle=53.174456 Des_Ro_dis=43.391057 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=53.174456 Robot_a=0\n",
      "0.759470489458\n",
      "278338\n",
      "**********************************\n",
      "last_x=854.570349, last_y=908.840475,last_a=0.000000\n",
      "x=866.130349 y=908.840475 True_angle=14.628656 Des_Ro_dis=35.004389 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-14.628656 Robot_a=0\n",
      "0.758888377067\n",
      "279115\n",
      "**********************************\n",
      "last_x=852.577014, last_y=908.956504,last_a=315.000000\n",
      "x=860.751169 y=900.782350 True_angle=43.858070 Des_Ro_dis=39.256628 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=-1.141930 Robot_a=315\n",
      "0.758543219091\n",
      "279576\n",
      "**********************************\n",
      "last_x=870.448863, last_y=861.421725,last_a=45.000000\n",
      "x=878.623018 y=869.595880 True_angle=9.889142 Des_Ro_dis=37.167000 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=54.889142 Robot_a=45\n",
      "0.75792069014\n",
      "280408\n",
      "**********************************\n",
      "last_x=858.795619, last_y=874.086765,last_a=45.000000\n",
      "x=866.969773 y=882.260919 True_angle=16.761712 Des_Ro_dis=37.492278 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=28.238288 Robot_a=45\n",
      "0.757362202006\n",
      "281155\n",
      "**********************************\n",
      "last_x=865.779680, last_y=863.648353,last_a=45.000000\n",
      "x=873.953834 y=871.822508 True_angle=2.250922 Des_Ro_dis=38.371523 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=47.250922 Robot_a=45\n",
      "0.756609987072\n",
      "282162\n",
      "**********************************\n",
      "last_x=858.983083, last_y=877.468419,last_a=0.000000\n",
      "x=870.543083 y=877.468419 True_angle=37.412356 Des_Ro_dis=37.086144 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=37.412356 Robot_a=0\n",
      "0.756094257776\n",
      "282853\n",
      "**********************************\n",
      "last_x=862.506876, last_y=871.993791,last_a=225.000000\n",
      "x=870.681030 y=880.167945 True_angle=169.075364 Des_Ro_dis=35.396503 Action=1 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=34.075364 Robot_a=225\n",
      "0.755667613893\n",
      "283425\n",
      "**********************************\n",
      "last_x=854.097232, last_y=905.909226,last_a=0.000000\n",
      "x=865.657232 y=905.909226 True_angle=9.763059 Des_Ro_dis=34.847449 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-9.763059 Robot_a=0\n",
      "0.755451401637\n",
      "283715\n",
      "**********************************\n",
      "last_x=855.141473, last_y=881.668881,last_a=45.000000\n",
      "x=863.315627 y=889.843035 True_angle=29.523950 Des_Ro_dis=38.064513 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=15.476050 Robot_a=45\n",
      "0.755110808178\n",
      "284172\n",
      "**********************************\n",
      "last_x=849.562522, last_y=900.472423,last_a=0.000000\n",
      "x=861.122522 y=900.472423 True_angle=0.696200 Des_Ro_dis=38.880348 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-0.696200 Robot_a=0\n",
      "0.754629622036\n",
      "284818\n",
      "**********************************\n",
      "last_x=858.904232, last_y=874.260056,last_a=45.000000\n",
      "x=867.078386 y=882.434210 True_angle=16.917222 Des_Ro_dis=37.314738 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=28.082778 Robot_a=45\n",
      "0.754388401118\n",
      "285142\n",
      "**********************************\n",
      "last_x=866.723225, last_y=870.270540,last_a=45.000000\n",
      "x=874.897380 y=878.444694 True_angle=4.347726 Des_Ro_dis=33.087350 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=40.652274 Robot_a=45\n",
      "0.753866023836\n",
      "285844\n",
      "**********************************\n",
      "last_x=855.512564, last_y=880.658633,last_a=0.000000\n",
      "x=867.072564 y=880.658633 True_angle=30.429706 Des_Ro_dis=38.187754 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=30.429706 Robot_a=0\n",
      "0.753349216561\n",
      "286539\n",
      "**********************************\n",
      "last_x=857.037969, last_y=875.766658,last_a=0.000000\n",
      "x=868.597969 y=875.766658 True_angle=37.657820 Des_Ro_dis=39.665380 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=37.657820 Robot_a=0\n",
      "0.753170834156\n",
      "286779\n",
      "**********************************\n",
      "last_x=855.537168, last_y=877.637006,last_a=45.000000\n",
      "x=863.711322 y=885.811161 True_angle=23.644548 Des_Ro_dis=38.963974 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=21.355452 Robot_a=45\n",
      "0.752702783982\n",
      "287409\n",
      "**********************************\n",
      "last_x=848.564479, last_y=895.561745,last_a=0.000000\n",
      "x=860.124479 y=895.561745 True_angle=6.351037 Des_Ro_dis=40.121756 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=6.351037 Robot_a=0\n",
      "0.752477779128\n",
      "287712\n",
      "**********************************\n",
      "last_x=871.637750, last_y=867.610774,last_a=135.000000\n",
      "x=863.463596 y=875.784929 True_angle=101.464953 Des_Ro_dis=43.832391 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=33.535047 Robot_a=135\n",
      "0.752068786559\n",
      "288263\n",
      "**********************************\n",
      "last_x=856.683622, last_y=866.926987,last_a=225.000000\n",
      "x=864.857776 y=875.101141 True_angle=170.318216 Des_Ro_dis=43.068887 Action=1 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=35.318216 Robot_a=225\n",
      "0.751571766993\n",
      "288933\n",
      "**********************************\n",
      "last_x=871.795320, last_y=869.444188,last_a=45.000000\n",
      "x=879.969475 y=877.618343 True_angle=3.172961 Des_Ro_dis=30.035987 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=48.172961 Robot_a=45\n",
      "0.751220345269\n",
      "289407\n",
      "**********************************\n",
      "last_x=884.949513, last_y=851.444959,last_a=90.000000\n",
      "x=884.949513 y=863.004959 True_angle=22.137693 Des_Ro_dis=39.939331 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=67.862307 Robot_a=90\n",
      "0.750892057377\n",
      "289850\n",
      "**********************************\n",
      "last_x=857.747337, last_y=881.406319,last_a=45.000000\n",
      "x=865.921491 y=889.580474 True_angle=27.998953 Des_Ro_dis=35.635815 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=17.001047 Robot_a=45\n",
      "0.75045505999\n",
      "290440\n",
      "**********************************\n",
      "last_x=885.136532, last_y=850.828693,last_a=90.000000\n",
      "x=885.136532 y=862.388693 True_angle=21.563245 Des_Ro_dis=40.441724 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=68.436755 Robot_a=90\n",
      "0.74983851775\n",
      "291273\n",
      "**********************************\n",
      "last_x=869.987872, last_y=858.494400,last_a=45.000000\n",
      "x=878.162026 y=866.668554 True_angle=11.768133 Des_Ro_dis=39.848242 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=56.768133 Robot_a=45\n",
      "0.74948126203\n",
      "291756\n",
      "**********************************\n",
      "last_x=870.899690, last_y=856.520565,last_a=45.000000\n",
      "x=879.073845 y=864.694720 True_angle=14.343918 Des_Ro_dis=41.041038 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=59.343918 Robot_a=45\n",
      "0.749220271213\n",
      "292109\n",
      "**********************************\n",
      "last_x=877.458406, last_y=850.150491,last_a=90.000000\n",
      "x=877.458406 y=861.710491 True_angle=30.485944 Des_Ro_dis=44.432083 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=59.514056 Robot_a=90\n",
      "0.748728853147\n",
      "292774\n",
      "**********************************\n",
      "last_x=869.813096, last_y=870.845582,last_a=45.000000\n",
      "x=877.987250 y=879.019737 True_angle=1.375706 Des_Ro_dis=30.409417 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=43.624294 Robot_a=45\n",
      "0.748633563271\n",
      "292903\n",
      "**********************************\n",
      "last_x=862.401585, last_y=870.236282,last_a=0.000000\n",
      "x=873.961585 y=870.236282 True_angle=48.819350 Des_Ro_dis=39.545897 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=48.819350 Robot_a=0\n",
      "0.748225950056\n",
      "293455\n",
      "**********************************\n",
      "last_x=877.613864, last_y=859.955119,last_a=90.000000\n",
      "x=877.613864 y=871.515119 True_angle=38.163640 Des_Ro_dis=36.228822 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=51.836360 Robot_a=90\n",
      "0.747748472347\n",
      "294102\n",
      "**********************************\n",
      "last_x=883.848003, last_y=855.633987,last_a=270.000000\n",
      "x=883.848003 y=867.193987 True_angle=153.786732 Des_Ro_dis=36.566672 Action=1 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=63.786732 Robot_a=270\n",
      "0.747061211084\n",
      "295034\n",
      "**********************************\n",
      "last_x=870.835682, last_y=872.157972,last_a=45.000000\n",
      "x=879.009837 y=880.332126 True_angle=1.862729 Des_Ro_dis=28.764774 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=43.137271 Robot_a=45\n",
      "0.746620580242\n",
      "295632\n",
      "**********************************\n",
      "last_x=862.678709, last_y=866.528188,last_a=45.000000\n",
      "x=870.852864 y=874.702343 True_angle=4.044325 Des_Ro_dis=38.594391 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=40.955675 Robot_a=45\n",
      "0.746023423131\n",
      "296443\n",
      "**********************************\n",
      "last_x=854.502688, last_y=887.179786,last_a=0.000000\n",
      "x=866.062688 y=887.179786 True_angle=20.694641 Des_Ro_dis=36.278079 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=20.694641 Robot_a=0\n",
      "0.745443665128\n",
      "297231\n",
      "**********************************\n",
      "last_x=887.291184, last_y=856.037589,last_a=90.000000\n",
      "x=887.291184 y=867.597589 True_angle=21.416017 Des_Ro_dis=34.805606 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=68.583983 Robot_a=90\n",
      "0.745017966498\n",
      "297810\n",
      "**********************************\n",
      "last_x=873.739698, last_y=853.211117,last_a=90.000000\n",
      "x=873.739698 y=864.771117 True_angle=36.701571 Des_Ro_dis=43.939477 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=53.298429 Robot_a=90\n",
      "0.744828356323\n",
      "298068\n",
      "**********************************\n",
      "last_x=846.210510, last_y=898.444051,last_a=0.000000\n",
      "x=857.770510 y=898.444051 True_angle=2.110113 Des_Ro_dis=42.258145 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=2.110113 Robot_a=0\n",
      "0.744402279451\n",
      "298648\n",
      "**********************************\n",
      "last_x=900.709370, last_y=850.487803,last_a=135.000000\n",
      "x=892.535216 y=858.661957 True_angle=55.236106 Des_Ro_dis=42.006628 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=79.763894 Robot_a=135\n",
      "0.744060861776\n",
      "299113\n",
      "**********************************\n",
      "last_x=849.804764, last_y=893.071753,last_a=45.000000\n",
      "x=857.978919 y=901.245908 True_angle=46.698299 Des_Ro_dis=42.039548 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=-1.698299 Robot_a=45\n",
      "0.743677048322\n",
      "299636\n",
      "**********************************\n",
      "last_x=860.894658, last_y=873.551311,last_a=0.000000\n",
      "x=872.454658 y=873.551311 True_angle=43.836446 Des_Ro_dis=38.187420 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=43.836446 Robot_a=0\n",
      "0.743254572027\n",
      "300212\n",
      "**********************************\n",
      "last_x=892.423467, last_y=856.596122,last_a=90.000000\n",
      "x=892.423467 y=868.156122 True_angle=13.383400 Des_Ro_dis=32.732803 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=76.616600 Robot_a=90\n",
      "0.742958397011\n",
      "300616\n",
      "**********************************\n",
      "last_x=860.564924, last_y=876.964724,last_a=225.000000\n",
      "x=868.739079 y=885.138879 True_angle=160.425935 Des_Ro_dis=34.613554 Action=1 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=25.425935 Robot_a=225\n",
      "0.742846995801\n",
      "300768\n",
      "**********************************\n",
      "last_x=879.946299, last_y=858.943016,last_a=45.000000\n",
      "x=888.120454 y=867.117170 True_angle=25.136784 Des_Ro_dis=34.962896 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=70.136784 Robot_a=45\n",
      "0.742480663894\n",
      "301268\n",
      "**********************************\n",
      "last_x=899.968735, last_y=849.075161,last_a=90.000000\n",
      "x=899.968735 y=860.635161 True_angle=0.045506 Des_Ro_dis=39.364851 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=89.954494 Robot_a=90\n",
      "0.742002510155\n",
      "301921\n",
      "**********************************\n",
      "last_x=854.767321, last_y=874.650182,last_a=45.000000\n",
      "x=862.941475 y=882.824336 True_angle=20.133502 Des_Ro_dis=40.845289 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=24.866498 Robot_a=45\n",
      "0.741943952296\n",
      "302001\n",
      "**********************************\n",
      "last_x=861.180787, last_y=871.230186,last_a=0.000000\n",
      "x=872.740787 y=871.230186 True_angle=46.544380 Des_Ro_dis=39.632902 Action=0 S_N=28\n",
      "L60D210/240\n",
      "Des_Robot_a=46.544380 Robot_a=0\n",
      "0.741507112167\n",
      "302598\n",
      "**********************************\n",
      "last_x=862.726557, last_y=867.156072,last_a=45.000000\n",
      "x=870.900712 y=875.330227 True_angle=4.709413 Des_Ro_dis=38.149263 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=40.290587 Robot_a=45\n",
      "0.7411677716\n",
      "303062\n",
      "**********************************\n",
      "last_x=885.330293, last_y=855.024348,last_a=45.000000\n",
      "x=893.504448 y=863.198503 True_angle=34.990253 Des_Ro_dis=37.370341 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=79.990253 Robot_a=45\n",
      "0.740989388427\n",
      "303306\n",
      "**********************************\n",
      "last_x=869.699726, last_y=859.405399,last_a=45.000000\n",
      "x=877.873880 y=867.579554 True_angle=10.687470 Des_Ro_dis=39.251121 Action=0 S_N=24\n",
      "L60D180/210\n",
      "Des_Robot_a=55.687470 Robot_a=45\n",
      "0.740182091267\n",
      "304411\n"
     ]
    }
   ],
   "source": [
    "def run2():\n",
    "    global Epoche\n",
    "    global Action_times\n",
    "    global Epoche_action_interval\n",
    "    Crash_=False\n",
    "    Arrive_=False\n",
    "    Destination_x_,Destination_y_=Set_destination(900,900)\n",
    "    Current_x_,Current_y_,Current_a_=Random_start()\n",
    "    Epoche_false=0\n",
    "    Vec_x_=[]\n",
    "    Vec_y_=[]\n",
    "    Vec_x_.append(Current_x_)\n",
    "    Vec_y_.append(Current_y_)\n",
    "    while Epoche<100:\n",
    "        True_a_,Le_ri_,Distance_=Robot_desti_(Destination_x_,Destination_y_,Current_x_,Current_y_,Current_a_)\n",
    "        Current_state_number_,Dis_level_,Angle_level_=Output_state_index(True_a_,Le_ri_,Distance_)\n",
    "        Next_action_=Choose_action2(Q_Table2,Current_state_number_,Action_times)\n",
    "        Next_x,Next_y,Next_a,R_,Next_state_number_,Arrive_,Crash_=Next_state_F2(Current_x_,Current_y_,Current_a_,Destination_x_,Destination_y_,Next_action_)\n",
    "#         print('%f  %f'%(Next_x,Next_y))\n",
    "        \n",
    "        if Crash_==True:\n",
    "            Next_x,Next_y,Next_a=Random_start()\n",
    "#             print('Run Boom!')\n",
    "        else:\n",
    "            if Arrive_==True:\n",
    "                Next_True_a,Next_lr,Next_Dis=Robot_desti_(Destination_x_,Destination_y_,Next_x,Next_y,Next_a)\n",
    "                print('**********************************')\n",
    "                print('last_x=%f, last_y=%f,last_a=%f'%(Current_x_,Current_y_,Current_a_%360))\n",
    "                print('x=%f y=%f True_angle=%f Des_Ro_dis=%f Action=%d S_N=%d'%(Next_x,Next_y,Next_True_a,Next_Dis,Next_action_,Next_state_number_))\n",
    "                print(States2[Next_state_number_])\n",
    "                print('Des_Robot_a=%f Robot_a=%d'%((np.arctan((Next_y-Destination_y_)/(Next_x-Destination_x_)))*180/np.pi,Next_a%360))\n",
    "                print(Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_Rate*Action_times))\n",
    "                print(Action_times)\n",
    "                Next_x,Next_y,Next_a=Random_start()\n",
    "                Epoche+=1\n",
    "#                 Plot_Move(Vec_x_,Vec_y_,Destination_x_,Destination_y_)\n",
    "            Q_target=R_+Beta*max(Q_Table2[Next_state_number_])\n",
    "            Q_Table2[Current_state_number_][Next_action_]+=Alpha*(Q_target-Q_Table2[Current_state_number_][Next_action_])\n",
    "            Vec_x_.append(Current_x_)\n",
    "            Vec_y_.append(Current_y_)\n",
    "        Current_x_=Next_x\n",
    "        Current_y_=Next_y\n",
    "        Current_a_=Next_a\n",
    "        Action_times+=1\n",
    "    return Q_Table2\n",
    "Q_Table2_final=run2()\n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 495,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style>\n",
       "    .dataframe thead tr:only-child th {\n",
       "        text-align: right;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: left;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>Up</th>\n",
       "      <th>Down</th>\n",
       "      <th>Left_45D</th>\n",
       "      <th>Right_45D</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>L60D0/30</th>\n",
       "      <td>2007.290170</td>\n",
       "      <td>1703.161482</td>\n",
       "      <td>1243.726743</td>\n",
       "      <td>1213.728718</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D0/30</th>\n",
       "      <td>-10.000000</td>\n",
       "      <td>-10.000000</td>\n",
       "      <td>-10.406773</td>\n",
       "      <td>-10.688663</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D0/30</th>\n",
       "      <td>-20.000000</td>\n",
       "      <td>-20.000000</td>\n",
       "      <td>-20.230142</td>\n",
       "      <td>-20.713402</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D0/30</th>\n",
       "      <td>-30.000000</td>\n",
       "      <td>-30.000000</td>\n",
       "      <td>-30.053660</td>\n",
       "      <td>-31.138218</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D30/60</th>\n",
       "      <td>956.366211</td>\n",
       "      <td>873.583211</td>\n",
       "      <td>1568.474799</td>\n",
       "      <td>629.881468</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D30/60</th>\n",
       "      <td>-10.500000</td>\n",
       "      <td>-10.500000</td>\n",
       "      <td>-10.000000</td>\n",
       "      <td>-11.642623</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D30/60</th>\n",
       "      <td>-20.500000</td>\n",
       "      <td>-20.500000</td>\n",
       "      <td>-20.000000</td>\n",
       "      <td>-21.878154</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D30/60</th>\n",
       "      <td>-30.500000</td>\n",
       "      <td>-30.500000</td>\n",
       "      <td>-30.000000</td>\n",
       "      <td>-31.451512</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D60/90</th>\n",
       "      <td>782.910039</td>\n",
       "      <td>417.287239</td>\n",
       "      <td>1358.259075</td>\n",
       "      <td>387.314776</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D60/90</th>\n",
       "      <td>-11.175726</td>\n",
       "      <td>-11.179347</td>\n",
       "      <td>-10.190979</td>\n",
       "      <td>-13.011154</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D60/90</th>\n",
       "      <td>-21.214681</td>\n",
       "      <td>-21.251287</td>\n",
       "      <td>-20.235531</td>\n",
       "      <td>-22.579150</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D60/90</th>\n",
       "      <td>-31.347598</td>\n",
       "      <td>-31.398731</td>\n",
       "      <td>-30.130961</td>\n",
       "      <td>-33.281031</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D90/120</th>\n",
       "      <td>231.891455</td>\n",
       "      <td>128.823288</td>\n",
       "      <td>1038.022188</td>\n",
       "      <td>42.792106</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D90/120</th>\n",
       "      <td>-12.254560</td>\n",
       "      <td>-12.301221</td>\n",
       "      <td>-10.774159</td>\n",
       "      <td>-13.988282</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D90/120</th>\n",
       "      <td>-22.137131</td>\n",
       "      <td>-22.091704</td>\n",
       "      <td>-20.581676</td>\n",
       "      <td>-23.852673</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D90/120</th>\n",
       "      <td>-32.057972</td>\n",
       "      <td>-32.095521</td>\n",
       "      <td>-30.616981</td>\n",
       "      <td>-33.550698</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D120/150</th>\n",
       "      <td>188.831871</td>\n",
       "      <td>19.156151</td>\n",
       "      <td>99.254210</td>\n",
       "      <td>76.330971</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D120/150</th>\n",
       "      <td>-13.408056</td>\n",
       "      <td>-13.364117</td>\n",
       "      <td>-11.452666</td>\n",
       "      <td>-15.024423</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D120/1500</th>\n",
       "      <td>-23.643894</td>\n",
       "      <td>-23.632399</td>\n",
       "      <td>-21.906553</td>\n",
       "      <td>-25.000392</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D120/150</th>\n",
       "      <td>-33.434097</td>\n",
       "      <td>-33.379812</td>\n",
       "      <td>-31.459789</td>\n",
       "      <td>-35.357759</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D150/180</th>\n",
       "      <td>87.547178</td>\n",
       "      <td>30.133733</td>\n",
       "      <td>35.288045</td>\n",
       "      <td>42.160335</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D150/180</th>\n",
       "      <td>-14.818894</td>\n",
       "      <td>-14.741480</td>\n",
       "      <td>-12.451661</td>\n",
       "      <td>-14.228146</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D150/180</th>\n",
       "      <td>-24.870492</td>\n",
       "      <td>-24.801776</td>\n",
       "      <td>-22.724736</td>\n",
       "      <td>-24.924063</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D150/180</th>\n",
       "      <td>-35.321809</td>\n",
       "      <td>-35.241717</td>\n",
       "      <td>-33.308577</td>\n",
       "      <td>-33.896689</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D180/210</th>\n",
       "      <td>1563.258315</td>\n",
       "      <td>1281.042970</td>\n",
       "      <td>977.470249</td>\n",
       "      <td>1423.192230</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D180/210</th>\n",
       "      <td>-10.000000</td>\n",
       "      <td>-10.000000</td>\n",
       "      <td>-10.844156</td>\n",
       "      <td>-10.360704</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D180/210</th>\n",
       "      <td>-20.000000</td>\n",
       "      <td>-20.000000</td>\n",
       "      <td>-20.643115</td>\n",
       "      <td>-20.345594</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D180/210</th>\n",
       "      <td>-30.000000</td>\n",
       "      <td>-30.000000</td>\n",
       "      <td>-30.933157</td>\n",
       "      <td>-30.163840</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D210/240</th>\n",
       "      <td>1307.389827</td>\n",
       "      <td>1094.585259</td>\n",
       "      <td>758.184480</td>\n",
       "      <td>1596.040934</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D210/240</th>\n",
       "      <td>-10.500000</td>\n",
       "      <td>-10.500000</td>\n",
       "      <td>-11.582609</td>\n",
       "      <td>-10.000000</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D210/240</th>\n",
       "      <td>-20.500000</td>\n",
       "      <td>-20.500000</td>\n",
       "      <td>-21.926630</td>\n",
       "      <td>-20.000000</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D210/240</th>\n",
       "      <td>-30.500000</td>\n",
       "      <td>-30.500000</td>\n",
       "      <td>-31.960893</td>\n",
       "      <td>-30.000000</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D240/270</th>\n",
       "      <td>103.549507</td>\n",
       "      <td>601.132001</td>\n",
       "      <td>265.783508</td>\n",
       "      <td>1232.992260</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D240/270</th>\n",
       "      <td>-11.194455</td>\n",
       "      <td>-11.182482</td>\n",
       "      <td>-12.617840</td>\n",
       "      <td>-10.375347</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D240/270</th>\n",
       "      <td>-21.229338</td>\n",
       "      <td>-21.224530</td>\n",
       "      <td>-22.734608</td>\n",
       "      <td>-20.143475</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D240/270</th>\n",
       "      <td>-31.256599</td>\n",
       "      <td>-31.187237</td>\n",
       "      <td>-32.550458</td>\n",
       "      <td>-30.067009</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D270/300</th>\n",
       "      <td>180.265214</td>\n",
       "      <td>95.136070</td>\n",
       "      <td>177.123115</td>\n",
       "      <td>629.837384</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D270/300</th>\n",
       "      <td>-12.225216</td>\n",
       "      <td>-12.190959</td>\n",
       "      <td>-14.106246</td>\n",
       "      <td>-10.808274</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D270/300</th>\n",
       "      <td>-22.149613</td>\n",
       "      <td>-22.189498</td>\n",
       "      <td>-23.605581</td>\n",
       "      <td>-20.830544</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D270/300</th>\n",
       "      <td>-32.074306</td>\n",
       "      <td>-31.993299</td>\n",
       "      <td>-34.324067</td>\n",
       "      <td>-30.905611</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D300/330</th>\n",
       "      <td>224.793138</td>\n",
       "      <td>100.617274</td>\n",
       "      <td>180.232354</td>\n",
       "      <td>440.606831</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D300/330</th>\n",
       "      <td>-13.656134</td>\n",
       "      <td>-13.665069</td>\n",
       "      <td>-14.906686</td>\n",
       "      <td>-11.802038</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D300/330</th>\n",
       "      <td>-23.465350</td>\n",
       "      <td>-23.420420</td>\n",
       "      <td>-24.928129</td>\n",
       "      <td>-21.684246</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D300/330</th>\n",
       "      <td>-33.697416</td>\n",
       "      <td>-33.661311</td>\n",
       "      <td>-35.396638</td>\n",
       "      <td>-31.917590</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D330/360</th>\n",
       "      <td>34.866154</td>\n",
       "      <td>557.014184</td>\n",
       "      <td>30.870248</td>\n",
       "      <td>38.089274</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D330/360</th>\n",
       "      <td>-14.972977</td>\n",
       "      <td>-14.973579</td>\n",
       "      <td>-14.469895</td>\n",
       "      <td>-12.504243</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D330/360</th>\n",
       "      <td>-25.113620</td>\n",
       "      <td>-25.131493</td>\n",
       "      <td>-24.545857</td>\n",
       "      <td>-22.773045</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L30D330/360</th>\n",
       "      <td>-35.321357</td>\n",
       "      <td>-35.320706</td>\n",
       "      <td>-34.101324</td>\n",
       "      <td>-32.980476</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>"
      ],
      "text/plain": [
       "                        Up         Down     Left_45D    Right_45D\n",
       "L60D0/30       2007.290170  1703.161482  1243.726743  1213.728718\n",
       "L180D0/30       -10.000000   -10.000000   -10.406773   -10.688663\n",
       "L250D0/30       -20.000000   -20.000000   -20.230142   -20.713402\n",
       "L300D0/30       -30.000000   -30.000000   -30.053660   -31.138218\n",
       "L60D30/60       956.366211   873.583211  1568.474799   629.881468\n",
       "L180D30/60      -10.500000   -10.500000   -10.000000   -11.642623\n",
       "L250D30/60      -20.500000   -20.500000   -20.000000   -21.878154\n",
       "L300D30/60      -30.500000   -30.500000   -30.000000   -31.451512\n",
       "L60D60/90       782.910039   417.287239  1358.259075   387.314776\n",
       "L180D60/90      -11.175726   -11.179347   -10.190979   -13.011154\n",
       "L250D60/90      -21.214681   -21.251287   -20.235531   -22.579150\n",
       "L300D60/90      -31.347598   -31.398731   -30.130961   -33.281031\n",
       "L60D90/120      231.891455   128.823288  1038.022188    42.792106\n",
       "L180D90/120     -12.254560   -12.301221   -10.774159   -13.988282\n",
       "L250D90/120     -22.137131   -22.091704   -20.581676   -23.852673\n",
       "L300D90/120     -32.057972   -32.095521   -30.616981   -33.550698\n",
       "L60D120/150     188.831871    19.156151    99.254210    76.330971\n",
       "L180D120/150    -13.408056   -13.364117   -11.452666   -15.024423\n",
       "L250D120/1500   -23.643894   -23.632399   -21.906553   -25.000392\n",
       "L300D120/150    -33.434097   -33.379812   -31.459789   -35.357759\n",
       "L60D150/180      87.547178    30.133733    35.288045    42.160335\n",
       "L180D150/180    -14.818894   -14.741480   -12.451661   -14.228146\n",
       "L250D150/180    -24.870492   -24.801776   -22.724736   -24.924063\n",
       "L300D150/180    -35.321809   -35.241717   -33.308577   -33.896689\n",
       "L60D180/210    1563.258315  1281.042970   977.470249  1423.192230\n",
       "L180D180/210    -10.000000   -10.000000   -10.844156   -10.360704\n",
       "L250D180/210    -20.000000   -20.000000   -20.643115   -20.345594\n",
       "L300D180/210    -30.000000   -30.000000   -30.933157   -30.163840\n",
       "L60D210/240    1307.389827  1094.585259   758.184480  1596.040934\n",
       "L180D210/240    -10.500000   -10.500000   -11.582609   -10.000000\n",
       "L250D210/240    -20.500000   -20.500000   -21.926630   -20.000000\n",
       "L300D210/240    -30.500000   -30.500000   -31.960893   -30.000000\n",
       "L60D240/270     103.549507   601.132001   265.783508  1232.992260\n",
       "L180D240/270    -11.194455   -11.182482   -12.617840   -10.375347\n",
       "L250D240/270    -21.229338   -21.224530   -22.734608   -20.143475\n",
       "L300D240/270    -31.256599   -31.187237   -32.550458   -30.067009\n",
       "L60D270/300     180.265214    95.136070   177.123115   629.837384\n",
       "L180D270/300    -12.225216   -12.190959   -14.106246   -10.808274\n",
       "L250D270/300    -22.149613   -22.189498   -23.605581   -20.830544\n",
       "L300D270/300    -32.074306   -31.993299   -34.324067   -30.905611\n",
       "L60D300/330     224.793138   100.617274   180.232354   440.606831\n",
       "L180D300/330    -13.656134   -13.665069   -14.906686   -11.802038\n",
       "L250D300/330    -23.465350   -23.420420   -24.928129   -21.684246\n",
       "L300D300/330    -33.697416   -33.661311   -35.396638   -31.917590\n",
       "L60D330/360      34.866154   557.014184    30.870248    38.089274\n",
       "L180D330/360    -14.972977   -14.973579   -14.469895   -12.504243\n",
       "L250D330/360    -25.113620   -25.131493   -24.545857   -22.773045\n",
       "L30D330/360     -35.321357   -35.320706   -34.101324   -32.980476"
      ]
     },
     "execution_count": 495,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Q_Table2_=pd.DataFrame(Q_Table2_final,columns=Actions2,index=States2)\n",
    "Q_Table2_"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
